#include "uav_swarms/leader.hpp"
#include "rclcpp/rclcpp.hpp"
#include <algorithm>
#include <vector>
#include <algorithm>  
#include <cstring>  // For strlen
#include <Eigen/Eigen>
#include <Eigen/Core>
/**
 * @brief Leader 类的构造函数
 *
 * 初始化 Leader 对象，并设置其属性。
 * 初始化编队生成器，设置避障速度订阅，订阅无人机位姿、速度等主题，
 * 创建与 MAVROS 交互的服务客户端，发布领导者的位置信息、编队模式信息、
 * 通信拓扑信息和无人机控制速度主题。
 */
Leader::Leader(): Node("leader") {

        initialize_parameters();


        namespaces = namespaces + "/";

        //  初始化无人机的目标位姿
        target_pose_ = geometry_msgs::msg::Pose();
        target_pose_.position.x = 0.0;
        target_pose_.position.y = 0.0;
        target_pose_.position.z = 3.0;
        target_pose_.orientation.x = 0.0;
        target_pose_.orientation.y = 0.0;
        target_pose_.orientation.z = 0.0;
        target_pose_.orientation.w = 1.0;

        // 初始化无人机的控制模式为 1
        control_flag = 1;
        
        avoid_vel = geometry_msgs::msg::Vector3();
        generator_queue = std::make_shared<Generator_queue>();
        generator_queue->set_property(uav_num);
        generator_queue->generate_formation();

        formation_dict.clear();
        
        formation_dict = generator_queue->get_queue();
        for (auto &i : formation_dict) {
            formation_dicts.push_back(i.first);
            auto &formation = i.second;
            std::cout << "formation size: " << formation.size() << std::endl;
            std::cout << "formation name is : " << i.first<< std::endl;
            for (auto &j : i.second) {
                std::cout << j.transpose() <<std::endl;

            }
        }
        control_flag = 1;
        origin_formation = formation_dict["origin"];
        // 实现避障速度的订阅
        avoid_vel_sub_ = this->create_subscription<geometry_msgs::msg::Vector3>(
            namespaces + uav_type + "_" + std::to_string(uav_id) + "/avoid_vel",
            rclcpp::QoS(10).best_effort(), std::bind(&Leader::avoid_vel_callback, this, std::placeholders::_1));
        
        // 订阅无人机的位姿
        pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            uav_type + "_" + std::to_string(uav_id) + "/vision_pose/pose",
            rclcpp::QoS(10).best_effort(), std::bind(&Leader::pose_callback, this, std::placeholders::_1));

        target_pose_sub_ = this->create_subscription<geometry_msgs::msg::Pose>(
            namespaces+ "leader/pose",
            rclcpp::QoS(10).best_effort(), std::bind(&Leader::target_pose_callback, this, std::placeholders::_1));
        
        // 订阅无人机的速度
        leader_velocity_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
            uav_type + "_" + std::to_string(uav_id) + "/local_position/velocity_local",
            rclcpp::QoS(10).best_effort(), std::bind(&Leader::leader_velocity_callback, this, std::placeholders::_1));
        
        leader_velocity_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(namespaces + "leader/velocity", 10);

        // 订阅无人机的速度,由于用户控制速度
        cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
            namespaces + "leader/cmd_vel_flu", rclcpp::QoS(10).best_effort(),
            std::bind(&Leader::cmd_vel_callback, this, std::placeholders::_1));

        // 获得当前的命令
        leader_cmd_sub_ = this->create_subscription<std_msgs::msg::String>(
            namespaces + "leader/cmd", 
            rclcpp::QoS(10).best_effort(), std::bind(&Leader::cmd_callback, this, std::placeholders::_1));
        

        // ROS 服务客户端：用于与 MAVROS 交互，控制无人机的解锁（arming）状态
        arming_client_ = this->create_client<mavros_msgs::srv::CommandBool>(
            uav_type + "_" + std::to_string(uav_id) + "/cmd/arming");
        
        // ROS 服务客户端：用于与 MAVROS 交互，切换无人机的飞行模式
        set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>(
            uav_type + "_" + std::to_string(uav_id) + "/set_mode");



        // 发布领导者的位置信息主题
        pose_enu_pub_ = this->create_publisher<geometry_msgs::msg::Pose>(namespaces + uav_type+"_" + std::to_string(uav_id) +"/cmd_pose_enu", 10);
        // 发布编队的模式主题信息
        formation_pattern_pub_ = this->create_publisher<std_msgs::msg::Float32MultiArray>(namespaces +"formation_pattern", 10);
        
        // 发布通信拓扑主题
        communication_topology_pub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>(  namespaces + "communication_topology", 10);
        
        // 发布无人机控制速度主题
        vel_flu_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(namespaces + uav_type+"_" + std::to_string(uav_id) + "/cmd_vel_flu", 10);
        
        state_sub_ = this->create_subscription<mavros_msgs::msg::State>(
            uav_type  +"_" + std::to_string(uav_id) +"/state", rclcpp::QoS(10).best_effort(), std::bind(&Leader::state_callback, this, std::placeholders::_1));
        // 发布字符串命令主题
        cmd_pub_ = this->create_publisher<std_msgs::msg::String>( namespaces +uav_type  +"_" + std::to_string(uav_id) + "/cmd", 10);

        server_status_sub_ = this->create_subscription<yolo_detect_message::msg::CallServerStatus>(
            namespaces + uav_type  +"_" + std::to_string(uav_id) + "Leader_server_topic", rclcpp::QoS(10).best_effort(), std::bind(&Leader::server_status_callback, this, std::placeholders::_1));

        adj_matrix_ = Eigen::MatrixXi::Zero(uav_num - 1, uav_num - 1);

        changed_id = std::vector<int>(uav_num - 1, 0);
        for (size_t i = 0; i < uav_num - 1; i++)
        {
            changed_id[i] = i;
        }
        new_formation = origin_formation;
        target_height_recorded = false;
        slack_right_.resize(uav_num - 1);
        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&Leader::control_loop, this));
        
    }
void Leader::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
        pose_ = *msg;
}

// 回调函数，用于处理避障速度
void Leader::avoid_vel_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) {
    avoid_velocity_ = *msg;
}
void Leader::target_pose_callback(const geometry_msgs::msg::Pose::SharedPtr msg){
    target_pose_ = *msg;
}
// 速度指令回调函数
void Leader::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
    cmd_vel_enu_ = *msg;
    
}
void Leader::cmd_callback(const std_msgs::msg::String::SharedPtr msg){
    std_msgs::msg::String msg_data = *msg;


    if ( std::find(formation_dicts.begin(),formation_dicts.end(),msg_data.data) != formation_dicts.end() && 
             msg_data.data != formation_config){
        formation_config = msg_data.data;

        


        FormationMatcher formation_matcher(uav_num);
        formation_matcher.set_values(origin_formation,formation_dict[formation_config]);
        formation_matcher.init();
        auto changed_id = formation_matcher.match();

        for (auto value:changed_id)
        {
            RCLCPP_INFO(this->get_logger(), "change id value: %d", value);
        }
       
        
        new_formation = get_new_formation(changed_id, formation_dict[formation_config]);
        communication_topology_ =get_communication_topology(new_formation);

        origin_formation = new_formation;
        for(auto value:origin_formation)
        {
            std::cout << value.transpose() << std::endl;
        }



    }else{

        cmd.data = msg->data;
      RCLCPP_INFO(this->get_logger(), "cmd.data: %s", formation_config.c_str());
    }
}

 Eigen::MatrixXi Leader::build_graph(std::vector<Eigen::Vector3d> origin_formation, std::vector<Eigen::Vector3d> change_formation){
    Eigen::MatrixXi adj_matrix(uav_num, uav_num);
   
    for (int i = 0; i < uav_num ; ++i){
        for (int j = 0; j < uav_num ; ++j){
            RCLCPP_INFO(this->get_logger(), "origin_formation[i].x(): %f,%f,%f", origin_formation[i].x(), origin_formation[i].y(), origin_formation[i].z());
            RCLCPP_INFO(this->get_logger(), "change_formation[j].x(): %f,%f,%f", change_formation[j].x(), change_formation[j].y(), change_formation[j].z());
            



            auto result = calculation_norm(origin_formation[i], change_formation[j]);
            Eigen::Vector3d  difference = origin_formation[i] - change_formation[j];
            RCLCPP_INFO(this->get_logger(), "result: %f", result);

            
            adj_matrix(i, j) = result;

            
          
        }
    }
    return adj_matrix;
}



bool Leader::find_path(int i) {
        visit_left_[i] = true;
        for (int j = 0; j < uav_num; ++j) {
            if (visit_right_[j])
            {
                continue;
            }
            int gap = label_left_[i] + label_right_[j] - adj_matrix_(i, j);
            
            if (gap == 0) {
                visit_right_[j] = true;
                if (match_right[j] == -1 || find_path(match_right[j])) {
                    match_right[j] = i; 
                    return true;
                }
            } else {
                slack_right_[j] = std::min(slack_right_[j], static_cast<double>(gap));
            }
        }
        return false;
}


void Leader::state_callback(const mavros_msgs::msg::State::SharedPtr msg) {
        current_state_ = *msg;
    };

std::vector<int> Leader::KM() {

    for (size_t i = 0; i < uav_num; i++)
    {
        std::fill(slack_right_.begin(), slack_right_.end(), 50);

        while (true)
        {
            visit_left_ = std::vector<bool>(uav_num, false);
            visit_right_ = std::vector<bool>(uav_num, false);
        
            if (find_path(i)) {
                break;
            }

            double d = std::numeric_limits<double>::infinity();
            for (int j = 0; j < uav_num; ++j) {
                if (!visit_right_[j]) {
                    d = std::min(d, slack_right_[j]);
                }
            }
            for (int k = 0; k < uav_num; ++k) {
                    if (visit_left_[k]) {
                        label_left_[k] -= d;
                    }
                    if (visit_right_[k]) {
                        label_right_[k] += d;
                    } else {
                        slack_right_[k] -= d;
                    }
                }
        }
        
    }
    
        // 返回匹配结果
        return match_right;
    }

 std::vector<Eigen::Vector3d> Leader::get_new_formation(const std::vector<int>& changed_id, const std::vector<Eigen::Vector3d>& change_formation) {
        // 初始化新的队形矩阵，大小为 3x(uav_num - 1)
        std::vector<Eigen::Vector3d> new_formation(uav_num, Eigen::Vector3d(0.0, 0.0, 0.0));

        // 初始化位置矩阵，大小为 3x(uav_num - 1)
        std::vector<Eigen::Vector3d> position(uav_num, Eigen::Vector3d(0.0, 0.0, 0.0));

        // 将changed_id列表中的元素加1，因为无人机ID通常从1开始计数
        std::vector<int> changed_id_plus_one = changed_id;
        

        // 将change_formation中的位置信息复制到position矩阵中
        for (int i = 0; i < uav_num; ++i) {
            position[i] = change_formation[i];
        }

        // 遍历每一对无人机，找到被修改的无人机，并将其位置复制到新的队形矩阵中
        for (int i = 0; i < uav_num ; ++i) {
            for (int j = 0; j < uav_num; ++j) {
                // 如果当前无人机的ID等于changed_id中的ID，则将其位置复制到new_formation中
                if ((j) == changed_id_plus_one[i]) {
                    new_formation[i] = position[j];
                }
            }
        }

        // 返回新的队形矩阵
        return new_formation;
    }



 std::vector<std::vector<int>> Leader::get_communication_topology(const std::vector<Eigen::Vector3d>& rel_posi) {
        int c_num = uav_num / 2;
        std::vector<int> min_num_index_list(c_num, 0);
        std::vector<std::vector<int>> comm(uav_num);
        std::vector<std::vector<int>> communication(uav_num, std::vector<int>(uav_num, 0));
        std::vector<int> nodes_next;
        std::vector<int> node_flag = { uav_num - 1 };
        std::vector<int> node_mid_flag;

        std::vector<double> rel_d(uav_num - 1, 0);

        // Calculate relative distances
        for (int i = 0; i < uav_num - 1; ++i) {
            rel_d[i] = std::pow(rel_posi[0][i], 2) + std::pow(rel_posi[1][i], 2) + std::pow(rel_posi[2][i], 2);
        }

        auto c = rel_d;
        std::sort(c.begin(), c.end());
        int count = 0;

        for (int j = 0; j < c_num; ++j) {
            for (int i = 0; i < uav_num - 1; ++i) {
                if (rel_d[i] == c[j] && std::find(node_mid_flag.begin(), node_mid_flag.end(), i) == node_mid_flag.end()) {
                    min_num_index_list[count] = i;
                    node_mid_flag.push_back(i);
                    count++;
                    if (count == c_num) break;
                }
            }
            if (count == c_num) break;
        }

        for (int j = 0; j < c_num; ++j) {
            nodes_next.push_back(min_num_index_list[j]);
            comm[uav_num - 1].push_back(min_num_index_list[j]);
        }

        int size_ = node_flag.size();

        while (!nodes_next.empty() && size_ < (uav_num - 1)) {
            int next_node = nodes_next[0];
            nodes_next.erase(nodes_next.begin());
            min_num_index_list.assign(c_num, 0);
            node_mid_flag.clear();
            rel_d.assign(uav_num - 1, 0);

            for (int i = 0; i < uav_num - 1; ++i) {
                if (i == next_node || std::find(node_flag.begin(), node_flag.end(), i) != node_flag.end()) {
                    rel_d[i] = 2000; // Large distance to exclude this node
                } else {
                    rel_d[i] = std::pow(rel_posi[0][i] - rel_posi[0][next_node], 2) +
                               std::pow(rel_posi[1][i] - rel_posi[1][next_node], 2) +
                               std::pow(rel_posi[2][i] - rel_posi[2][next_node], 2);
                }
            }

            c = rel_d;
            std::sort(c.begin(), c.end());
            count = 0;

            for (int j = 0; j < c_num; ++j) {
                for (int i = 0; i < uav_num - 1; ++i) {
                    if (rel_d[i] == c[j] && std::find(node_mid_flag.begin(), node_mid_flag.end(), i) == node_mid_flag.end()) {
                        min_num_index_list[count] = i;
                        node_mid_flag.push_back(i);
                        count++;
                        if (count == c_num) break;
                    }
                }
                if (count == c_num) break;
            }

            node_flag.push_back(next_node);
            size_ = node_flag.size();

            for (int j = 0; j < c_num; ++j) {
                if (std::find(node_flag.begin(), node_flag.end(), min_num_index_list[j]) != node_flag.end()) {
                    continue;
                }
                if (std::find(nodes_next.begin(), nodes_next.end(), min_num_index_list[j]) == nodes_next.end()) {
                    nodes_next.push_back(min_num_index_list[j]);
                }
                comm[next_node].push_back(min_num_index_list[j]);
            }
        }

        for (int i = 0; i < uav_num; ++i) {
            for (int j = 0; j < uav_num - 1; ++j) {
                if (i == 0) {
                    communication[j + 1][i] = (std::find(comm[uav_num - 1].begin(), comm[uav_num - 1].end(), j) != comm[uav_num - 1].end()) ? 1 : 0;
                } else {
                    communication[j + 1][i] = (std::find(comm[i - 1].begin(), comm[i - 1].end(), j) != comm[i - 1].end() && i < j + 1) ? 1 : 0;
                }
            }
        }

        // Ensure no UAV is left without communication
        for (int i = 1; i < uav_num; ++i) {
            if (std::accumulate(communication[i].begin(), communication[i].end(), 0) == 0) {
                communication[i][0] = 1;
            }
        }

        return communication;
    }

    void Leader::control_loop(){

       if(server_status == false){
           return;
       }
        // origin_formation = new_formation;
        // 保持当前速度不变
        cmd_vel_enu_.linear.x = cmd_vel_enu_.linear.x;
        cmd_vel_enu_.linear.y = cmd_vel_enu_.linear.y;
        cmd_vel_enu_.linear.z = cmd_vel_enu_.linear.z;

        // 初始化并发布队形模式数据
        std_msgs::msg::Float32MultiArray formation_pattern_msg;
        for (size_t i = 0; i < uav_num; i++)
        {
            formation_pattern_msg.data.push_back((static_cast<float>(origin_formation[i].x())));
            formation_pattern_msg.data.push_back((static_cast<float>(origin_formation[i].y())));
            formation_pattern_msg.data.push_back((static_cast<float>(origin_formation[i].z())));

        }
        formation_pattern_pub_->publish(formation_pattern_msg);
        // RCLCPP_INFO(this->get_logger(),"formation_pattern_pub_ pblishe messages");

        // 判断通信拓扑结构是否存在并发布
        if (!communication_topology_.empty()) {
            std_msgs::msg::Int32MultiArray communication_topology_msg;
            for (size_t i = 0; i < communication_topology_.size(); ++i) {
                for (size_t j = 0; j < communication_topology_[i].size(); ++j) {
                    communication_topology_msg.data.push_back(static_cast<int32_t>(communication_topology_[i][j]));
                }
            }
            communication_topology_pub_->publish(communication_topology_msg);
        }

        vel_flu_pub_->publish(cmd_vel_enu_);

        
    }

double Leader::calculation_norm(Eigen::Vector3d a, Eigen::Vector3d b){

    return 1.0 * -1 * sqrt(pow((a.x() - b.x()), 2) + pow((a.y() - b.y()), 2) + pow((a.z() - b.z()), 2));

}


void Leader::server_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
    std::lock_guard<std::mutex> lock(server_status_mutex_);
    server_status = msg->status;
}

void Leader::leader_velocity_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg){
    std::lock_guard<std::mutex> lock(leader_velocity_mutex_);
    leader_velocity_ = *msg;
}